"""
动作解析器模块

该模块负责解析和执行动作指令。
"""

import logging
from typing import Dict, List, Optional, Union
from robot_core.protocols.servo_protocol import ServoProtocol
from .kinematics import Kinematics


class ActionParser:
    """动作解析器类"""

    def __init__(self, protocol: ServoProtocol, log_level: int = logging.INFO):
        """
        初始化动作解析器

        Args:
            protocol: 舵机协议实例
            log_level: 日志级别
        """
        # 配置日志
        logging.basicConfig(level=log_level)
        self.logger = logging.getLogger(__name__)

        self.protocol = protocol
        self.kinematics = Kinematics()

    def parse_command(self, cmd: str, params: List[Union[int, float, str]]) -> bool:
        """
        解析并执行命令

        Args:
            cmd: 命令类型
            params: 命令参数

        Returns:
            bool: 命令是否执行成功
        """
        try:
            self.logger.info(f"执行命令: {cmd}, 参数: {params}")
            if cmd == "move_servo":
                return self._move_servo(params)
            elif cmd == "move_leg":
                return self._move_leg(params)
            elif cmd == "turn":
                return self._turn(params)
            elif cmd == "stand":
                return self._stand()
            elif cmd == "sit":
                return self._sit()
            else:
                self.logger.error(f"未知命令: {cmd}")
                return False
        except Exception as e:
            import traceback

            traceback.print_exc()
            self.logger.error(f"执行命令时出错: {e}")
            return False

    def _move_servo(self, params: List[Union[int, float]]) -> bool:
        """
        移动单个舵机

        Args:
            params: [舵机ID, 目标位置, 运动时间]

        Returns:
            bool: 是否执行成功
        """
        if len(params) != 3:
            self.logger.error("舵机移动参数错误")
            return False

        servo_id, position, time_ms = params
        return self.protocol.set_servo_angle(servo_id, position, time_ms)

    def _move_leg(self, params: List[Union[int, float]]) -> bool:
        """
        移动单条腿

        Args:
            params: [腿编号, x, y, z, 运动时间]

        Returns:
            bool: 是否执行成功
        """
        if len(params) != 5:
            self.logger.error("腿移动参数错误")
            return False

        leg, x, y, z, time_ms = params
        # 计算逆运动学
        angles = self.kinematics.inverse_kinematics(leg, [x, y, z])
        # 转换为舵机位置
        servo_positions = self.kinematics.angles_to_servo_positions(leg, angles)
        # 移动三个舵机
        success = True
        for i, pos in enumerate(servo_positions):
            if not self.protocol.set_servo_angle(
                leg * 3 + i + 1, int(pos), int(time_ms)
            ):
                success = False
        return success

    def _turn(self, params: List[Union[int, float]]) -> bool:
        """
        转向

        Args:
            params: [角度, 运动时间]

        Returns:
            bool: 是否执行成功
        """
        if len(params) != 2:
            self.logger.error("转向参数错误")
            return False

        angle, time_ms = params
        if angle > 23:
            angle = 23
        elif angle < -23:
            angle = -23

        # 计算每条腿的位置
        positions = []
        for leg in range(6):
            pos = self.kinematics.get_point(leg, angle)
            positions.append(pos)

        # 移动所有腿
        success = True
        for leg, pos in enumerate(positions):
            if not self._move_leg([leg, *pos, time_ms]):
                success = False
        return success

    def _stand(self) -> bool:
        """
        站立姿势

        Returns:
            bool: 是否执行成功
        """
        # 设置所有腿到站立位置
        success = True
        for leg in range(6):
            if not self._move_leg([leg, 100, 100, -70, 1000]):
                success = False
        return success

    def _sit(self) -> bool:
        """
        坐下姿势

        Returns:
            bool: 是否执行成功
        """
        # 设置所有腿到坐下位置
        success = True
        for leg in range(6):
            if not self._move_leg([leg, 100, 100, 20, 1000]):
                success = False
        return success
